package com.testcolision.simulation;

import com.badlogic.gdx.math.Intersector;
import com.badlogic.gdx.math.Plane;
import com.badlogic.gdx.math.Quaternion;
import com.badlogic.gdx.math.Vector3;
import com.badlogic.gdx.math.collision.BoundingBox;
import com.badlogic.gdx.math.collision.Ray;
 

/*
 * Bane: thish is the classhh with and exshtended methods for the heightmap! 
 */

public class TerrainSimulation  {
	public Vehicle r;

	public static final int STATE_CLEAR = 0;
	public static final int STATE_COLLIDING = 1;
	public static final int STATE_PENETRATING = 2;
	public float collisionState = STATE_CLEAR;

	public int throttle;
	public int steering;
	public int brakes;
	public int handbrakes;

	public Ray ray;
	public Intersector intersector;
	public Plane collisionPlane;
	public BoundingBox bounds;

	public Vector3 gravity;

	public Vector3 planePoint;

	public Vector3 normal;
	public Terrain terrain;
	public Plane[] collisionPlanes;

	public TerrainSimulation() {
		r = new Vehicle(new Vector3(3, 4, 4), 1f);

		r.setLocation(new Vector3(40.2f, 22.2f, 22.2f), new Quaternion(0f, 20f, 0f, 1));

		gravity = new Vector3(0, -40, 0);

		ray = new Ray(new Vector3(0, 0, 0), new Vector3(0, 0, 0));
		intersector = new Intersector();

		planePoint = new Vector3(0, 0, 0);
		collisionPlane = new Plane(new Vector3(0, 1, 0), planePoint);

		normal = new Vector3().set(collisionPlane.normal);
		terrain = new Terrain(32, 32, 6, 10);
		collisionPlanes = new Plane[r.rigidBodyPoints.length];
		for (short i = 0; i < r.rigidBodyPoints.length; i++)
			collisionPlanes[i] = new Plane(new Vector3(0, 1, 0), 10);
	}

	Plane heightMapPlane;
	int[] positionCoords = new int[4];
	Vector3[] terrainVertices = new Vector3[4];

	public void update(float delta) {

		if (delta > 0.01) {
			delta = 0.01f;
		}
		float currentTime = 0;
		float targetTime = delta;

		r.setBrakes(brakes);
		r.setThrottle(throttle);
		r.setSteering(steering);
		castRays(r.sourceStateIndex);

		while (currentTime < delta) {

			computeForces(r.sourceStateIndex, delta);

			integrate(targetTime - currentTime);

			calculateVertices(r.targetStateIndex);
			setCollisionPlanes(r.targetStateIndex);
			checkCollisions(r.targetStateIndex);

			if (collisionState == STATE_PENETRATING) {
				// we simulated too far, so subdivide time and try again
				targetTime = targetTime * 0.5f;
				for (RaycastWheel wheel : r.raycastWheels) {
					// integrate total torque into wheel

					wheel.wheelSpeed -= wheel.integratedSpeed;
					wheel.wheelTorque -= wheel.integratedTorque;

				}
			} else {

				if (collisionState == STATE_COLLIDING) {

					int counter = 0;
					do {
						counter++;
						if (counter < 40)
							resolveCollisions(r.targetStateIndex, 0);
						else {

							resolveCollisions(r.targetStateIndex, distance);
						}
					} while (checkCollisions(r.targetStateIndex) == STATE_COLLIDING && counter < 40);

				}
				currentTime = delta;
				targetTime = delta;
				r.sourceStateIndex = r.sourceStateIndex == 1 ? 0 : 1;
				r.targetStateIndex = r.targetStateIndex == 1 ? 0 : 1;
				for (RaycastWheel wheel : r.raycastWheels) {
					wheel.wheelTorque = 0;
				}

			}
		}

	}

	public void integrate(float delta) {
		RigidBodyState source = r.rigidBodyStates[r.sourceStateIndex];
		RigidBodyState target = r.rigidBodyStates[r.targetStateIndex];

		target.linearVelocity.set(source.linearVelocity).add(source.force_accumulator.cpy().mul(r.inverseMass).mul(delta));
		target.position.set(source.position).add(source.linearVelocity.cpy().mul(delta));

		target.angularMomentum.set(source.angularMomentum).add(source.torque_accumulator.cpy().mul(delta));
		target.angularVelocity.set(target.angularMomentum).mul(r.inverseInertia);
		target.orientation.set(source.orientation).nor();
		target.spin.set(target.angularVelocity.x, target.angularVelocity.y, target.angularVelocity.z, 0).mul(0.5f).mul(target.orientation);
		target.orientation.w += target.spin.w * delta;
		target.orientation.x += target.spin.x * delta;
		target.orientation.y += target.spin.y * delta;
		target.orientation.z += target.spin.z * delta;

		target.translationMatrix.setToTranslation(target.position);
		target.orientationMatrix.set(target.orientation);
		target.localToWorldMatrix.set(target.translationMatrix).mul(target.orientationMatrix);
		target.worldToLocalMatrix.set(target.localToWorldMatrix).inv();

		for (RaycastWheel wheel : r.raycastWheels) {
			// integrate total torque into wheel
			wheel.integratedSpeed = wheel.wheelTorque / wheel.wheelInertia * delta;

			wheel.wheelSpeed += wheel.integratedSpeed;

		}
	}

	float Kdl = 0.004f; // linear damping factor
	float Kda = 0.001f; // angular damping factor
	float k_ext = 10f; // Hooke's spring constant
	float d_ext = 2f;
	float k_comp = 10f;
	float d_comp = 1f; // damping constant
	public Vector3 rayLength = new Vector3();
	float springForceMag;
	float dampForceMag;

	Vector3 springForce = new Vector3();
	float restLength = 4f;
	public float[] tire_comp = new float[4];

	public void computeForces(int stateIndex, float delta) {
		RigidBodyState currentState = r.rigidBodyStates[stateIndex];
		// clear forces
		currentState.force_accumulator.set(0, 0, 0);
		currentState.torque_accumulator.set(0, 0, 0);

		currentState.force_accumulator.add(gravity);

		for (int index = 0; index < 4; index++) {
			RaycastWheel wheel = r.raycastWheels[index];
			Vector3 point = currentState.boundingPoints[index];

			rayLength.set(point).sub(wheel.intersectionPoint);
			if (rayLength.len() < 4) {
				tire_comp[index] = rayLength.len() - restLength;
				wheel.state = RaycastWheel.WHEEL_STATE_GROUND;

				springForceMag = (relativeVelocity < -2 ? -k_comp : -k_ext) * (rayLength.len() - restLength);

				Vector3 pointVelocity = r.pointVelocity(point);
				relativeVelocity = pointVelocity.dot(rayLength.nor());

				dampForceMag = (relativeVelocity < -2 ? -d_comp : -d_ext) * relativeVelocity;

				springForce.set(rayLength.nor()).mul(springForceMag + dampForceMag);
				r.addForce(springForce, wheel.intersectionPoint);
			} else {
				tire_comp[index] = 0;
				wheel.state = RaycastWheel.WHEEL_STATE_AIR;
			}
		}

		for (int index = 0; index < 4; index++) {
			RaycastWheel wheel = r.raycastWheels[index];
			if (wheel.state != RaycastWheel.WHEEL_STATE_AIR) {

				Vector3 wheelVelocity = new Vector3().set(r.pointVelocity(currentState.boundingPoints[index])).mul(currentState.orientationMatrix.cpy().inv());

				Vector3 patchSpeed = new Vector3().set(wheel.frontVec.cpy().mul(wheel.wheelSpeed).mul(Vehicle.TYRE_RADIUS));

				// get velocity difference between ground and patch
				Vector3 velDifference = wheelVelocity.cpy().sub(patchSpeed);

				float dotDistance = velDifference.cpy().dot(wheel.sideVec);

				Vector3 sideVel = wheel.sideVec.cpy().mul(dotDistance);
				float forwardMag = velDifference.cpy().dot(wheel.frontVec);

				float forwardMagFriction = wheelVelocity.dot(wheel.frontVec);

				Vector3 forwardVel = wheel.frontVec.cpy().mul(forwardMag);

				// calculate super friction forces
				Vector3 superFrictionForce = new Vector3().set(wheel.frontVec).mul(-1).mul(forwardMagFriction);

				// calculate torque on wheel
				wheel.integratedTorque = forwardMag * Vehicle.TYRE_RADIUS;
				wheel.wheelTorque += wheel.integratedTorque;

				// calculate response force
				Vector3 responseForce = new Vector3().set(sideVel.mul(-1).mul(2));
				responseForce.sub(forwardVel).add(superFrictionForce);

				r.addForce(responseForce.mul(currentState.orientationMatrix), currentState.boundingPoints[index]);
			}
		}

		// damping
		currentState.force_accumulator.add(currentState.linearVelocity.cpy().mul(-Kdl));
		currentState.torque_accumulator.add(currentState.angularVelocity.cpy().mul(-Kda));
	}

	/*
	 * Terrain tutorial from GameDev forums by BlueChip :
	 * http://www.gamedev.net/topic/451312-heightmap--collision-detection/
	 */

	float stepSize;
	int terrainHeight;

	public Plane calculateCollisionPlane(Vector3 coords) {
		// check x, z coordinates
		stepSize = terrain.stepSize;
		terrainHeight = terrain.height + 1;
		byte[] heightMap = terrain.heightMap;
		// Gdx.app.log("", ""+coords);
		positionCoords[0] = (int) Math.floor(coords.x / stepSize);
		positionCoords[1] = (int) Math.ceil(coords.x / stepSize);
		positionCoords[2] = (int) Math.floor(coords.z / stepSize);
		positionCoords[3] = (int) Math.ceil(coords.z / stepSize);

		// Gdx.app.log("", "A " + positionCoords[0] + "- B " + positionCoords[1]
		// + "- C " + positionCoords[2] + "- D " + positionCoords[3]);
		// get vertices in terrain
		terrainVertices[0] = new Vector3();// A
		terrainVertices[0].x = positionCoords[0] * stepSize;
		terrainVertices[0].y = heightMap[positionCoords[0] + (positionCoords[2] * terrainHeight)];
		terrainVertices[0].z = positionCoords[2] * stepSize;

		terrainVertices[1] = new Vector3();// B
		terrainVertices[1].x = positionCoords[1] * stepSize;
		terrainVertices[1].y = heightMap[positionCoords[1] + (positionCoords[2] * terrainHeight)];
		terrainVertices[1].z = positionCoords[2] * stepSize;

		terrainVertices[2] = new Vector3();// C
		terrainVertices[2].x = positionCoords[0] * stepSize;
		terrainVertices[2].y = heightMap[positionCoords[0] + (positionCoords[3] * terrainHeight)];
		terrainVertices[2].z = positionCoords[3] * stepSize;

		terrainVertices[3] = new Vector3();// D
		terrainVertices[3].x = positionCoords[1] * stepSize;
		terrainVertices[3].y = heightMap[positionCoords[1] + (positionCoords[3] * terrainHeight)];
		terrainVertices[3].z = positionCoords[3] * stepSize;

		// get in which triangle: coefX > coefZ? ABD : ACD
		float coefX = terrainVertices[1].x - coords.x; // B
		float coefZ = terrainVertices[3].z - coords.y;

		// if(coefX > terrain.stepsize/2 && )
		// create plane change order of vertices to get normals right ABD : 031,
		// ACD 031x

		// Gdx.app.log("coefX:   "+coefX, "     coefZ:  "+coefZ);
		if (coefX >= terrain.stepSize / 2 && coefZ >= terrain.stepSize / 2) {
			// Gdx.app.log("", "ACD");
			collisionPlane = new Plane(terrainVertices[0], terrainVertices[2], terrainVertices[3]);
		} else {
			// Gdx.app.log("", "ABD");
			collisionPlane = new Plane(terrainVertices[0], terrainVertices[3], terrainVertices[1]);

		}
		return collisionPlane;
	}

 
	public void setCollisionPlanes(int targetStateIndex) {
		RigidBodyState currentState = r.rigidBodyStates[r.targetStateIndex];
		RaycastWheel[] raycastWheels = r.raycastWheels;

		for (int i = 0; i < 8; i++) {
			collisionPlanes[i].set(calculateCollisionPlane(currentState.boundingPoints[i].cpy()));

			if (i < 4) {
				ray.set(currentState.boundingPoints[i], new Vector3(0, -1, 0));
				if (!Intersector.intersectRayPlane(ray, collisionPlanes[i], raycastWheels[i].intersectionPoint)) {
					ray.direction.mul(-1);
					Intersector.intersectRayPlane(ray, collisionPlanes[i], raycastWheels[i].intersectionPoint);
				}
			}
		}
	}

	public void castRays(int stateIndex) {
		for (int i = 0; i < 4; i++) {
			RigidBodyState currentState = r.rigidBodyStates[stateIndex];
			RaycastWheel[] raycastWheels = r.raycastWheels;

			ray.set(currentState.boundingPoints[i], new Vector3(0, -1, 0));
			if (!Intersector.intersectRayPlane(ray, collisionPlane, raycastWheels[i].intersectionPoint)) {
				ray.direction.mul(-1);
				Intersector.intersectRayPlane(ray, collisionPlane, raycastWheels[i].intersectionPoint);
			}
		}
	}

	public void calculateVertices(int targetStateIndex) {
		RigidBodyState currentState = r.rigidBodyStates[targetStateIndex];
		for (int i = 0; i < currentState.boundingPoints.length; i++) {
			currentState.boundingPoints[i].set(r.rigidBodyPoints[i]).mul(currentState.localToWorldMatrix);
		}

	}

	public float distance;
	public int collisionPointIndex;
	public float relativeVelocity;
	public float depthEpsilon = 0.001f;
 
	public float checkCollisions(int stateIndex) {

		collisionState = STATE_CLEAR;
		RigidBodyState currentState = r.rigidBodyStates[stateIndex];

		for (int pIndex = 0; pIndex < 8 && collisionState != STATE_COLLIDING; pIndex++) {
			collisionPlane.set(collisionPlanes[pIndex]);

			Vector3 position = new Vector3().set(currentState.boundingPoints[pIndex]);

			distance = position.dot(collisionPlane.normal) + collisionPlane.d;
			// Gdx.app.log("", "" + collisionPlane.normal);
			// if (distance < -depthEpsilon) {
			// collisionState = STATE_PENETRATING;
			//
			// } else
			if (distance < depthEpsilon) {

				Vector3 pointVelocity = r.pointVelocity(position);
				relativeVelocity = pointVelocity.dot(collisionPlane.normal);

				if (relativeVelocity < 0) {

					collisionState = STATE_COLLIDING;

					collisionPointIndex = pIndex;
				}
			}

		}

		return collisionState;
	}
 
	public void resolveCollisions(int stateIndex, float depth) {
		RigidBodyState currentState = r.rigidBodyStates[stateIndex];

		// vector_3 Position =
		// Configuration.aBoundingVertices[CollidingCornerIndex];
		Vector3 collidingPoint = new Vector3().set(currentState.boundingPoints[collisionPointIndex]);
		Vector3 pointPosition = new Vector3().set(collidingPoint);

		// vector_3 R = Position - Configuration.CMPosition;

		Vector3 positionToCM = pointPosition.cpy().sub(currentState.position);

		// vector_3 Velocity = Configuration.CMVelocity +
		// CrossProduct(Configuration.AngularVelocity,R);

		Vector3 pointVelocity = r.pointVelocity(pointPosition);

		// real ImpulseNumerator = -(r(1) + Body.CoefficientOfRestitution) *
		// DotProduct(Velocity,CollisionNormal);

		float depth_force_ammount = 1f;
		float impulseNumerator = -(1 + r.coefficientRestitution) * pointVelocity.dot(collisionPlane.normal) + (depth * depth_force_ammount);

		// real ImpulseDenominator = Body.OneOverMass +
		// DotProduct(CrossProduct(Configuration.InverseWorldInertiaTensor *
		// CrossProduct(R,CollisionNormal),R),
		// CollisionNormal);
		float impulseDenominator = r.inverseMass + positionToCM.cpy().crs(collisionPlane.normal).mul(r.inverseInertia).crs(positionToCM).dot(collisionPlane.normal);
		Vector3 impulse = collisionPlane.normal.cpy().mul(impulseNumerator / impulseDenominator);

		// // apply impulse to primary quantities
		// Configuration.CMVelocity += Body.OneOverMass * Impulse;
		// Configuration.AngularMomentum += CrossProduct(R,Impulse);
		//
		// // compute affected auxiliary quantities
		// Configuration.AngularVelocity =
		// Configuration.InverseWorldInertiaTensor *
		// Configuration.AngularMomentum;

		currentState.linearVelocity.add(impulse.cpy().mul(r.inverseMass));
		currentState.angularMomentum.add(positionToCM.cpy().crs(impulse));

		currentState.angularVelocity.set(currentState.angularMomentum.cpy().mul(r.inverseInertia));

	}
}
